/*-----------------------------------------------------------------------------------*/
/* Nuvoton Electronics Corporation confidential                                      */
/*                                                                                   */
/* Copyright (c) 2008 by Nuvoton Electronics Corporation                             */
/* All rights reserved                                                               */
/*                                                                                   */
/*-----------------------------------------------------------------------------------*/
/* File Contents:                                                                    */
/*   MASS.C                                                                          */
/*                                                                                   */
/* This file contains:                                                               */
/*                                                                                   */
/* Project:                                                                          */
/*   1. Support both non-OS and eCos version.                                        */
/*   2. It's recommended that don't print any debug message.                         */
/*                                                                                   */
/*                                                                                   */
/*-----------------------------------------------------------------------------------*/

#ifdef ECOS 
#include "stdio.h"
#include "string.h"
#include "stdlib.h"
#include "drv_api.h"
#include "diag.h"
#include "wbio.h"
#else
#include <stdio.h>
#include <string.h>
#include "wblib.h"
#endif

#include "nuc900_reg.h"
#include "nuc900_udc.h"
#include "nuc900_sic.h"
#include "ide.h"


#ifdef MASS_DEBUG
#ifdef ECOS
#define MDEBUG		diag_printf
#else
#define MDEBUG		sysprintf
#endif  /* ECOS */
#else
#define MDEBUG(...)
#endif  /* MASS_DEBUG */

#ifdef TEST_HD
extern int bScatterGatherUsed;
#define PIO_MODE	0
#define DMA_MODE	1
#define UDMA_MODE	2

#define MODE_0	    0
#define MODE_1		1
#define MODE_2		2
#define MODE_3		3
#define MODE_4		4

#define CLK_33MHZ	0
#define CLK_66MHZ	1
#endif  /* end of TEST_HD */

#ifdef TEST_RAM
UINT32 volatile Mass_Base_Addr, Storage_Base_Addr;
#else
/* Mass_Storage command base address */
#define MASS_BUFFER_SIZE 0x1000
#define STORAGE_BUFFER_SIZE 0x10000
__align(4096) UINT8 Mass_Block[MASS_BUFFER_SIZE];
__align(4096) UINT8 Storage_Block[STORAGE_BUFFER_SIZE];
#define Mass_Base_Addr  ((UINT32)&Mass_Block[0] | 0x80000000)
#define Storage_Base_Addr  ((UINT32)&Storage_Block[0] | 0x80000000)

#endif

#define	SizePerSector		512

UINT8 volatile Mass_LUN=0;
UINT8 volatile F_SD0_LUN=0xff;
UINT8 volatile F_SD1_LUN=0xff;
UINT8 volatile F_MS0_LUN=0xff;
UINT8 volatile F_MS1_LUN=0xff;
UINT8 volatile F_SM_LUN=0xff;
UINT8 volatile F_HD_LUN=0xff;

Disk_Par_Info DDB;

#ifdef TEST_RAM
UINT8 Flash_Buffer[512];
UINT8 StartSect[12] = {0,0x3b,0x1b,0x19,0x29,0x23,0x37,0x2f,0x5f,0xe1,0xe3,0xe3};
UINT8 FATSects[12] = {0,10,2,3,3,6,12,32,64,127,254,254};
UINT8 PartBootSector[0x13] = {
		0xe9,0x00,0x00,
		0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,
		0x00,0x02,
		0x00,
		0x01,0x00,
		0x02,
		0x00,0x01,
};
#endif

UINT8 volatile preventflag=0;
UINT8 volatile Bulk_First_Flag=0;
UINT8 volatile sd0_card_remove_flag=0;
UINT8 volatile sd0_card_Insert_flag=0;
UINT8 volatile sd1_card_remove_flag=0;
UINT8 volatile sd1_card_Insert_flag=0;
UINT8 volatile no_card_flag=0;
UINT8 errorFlagCBW=0;

int volatile gTotalSectors_SD0, gTotalSectors_SD1, gTotalSectors_SM, gTotalSectors_HD;
int volatile gTotalSectors_MS0, gTotalSectors_MS1, gTotalSectors_RAM;
int volatile Card_STS;

void DeviceInsert_sd0(void);
void DeviceRemove_sd0(void);
void DeviceInsert_sd1(void);
void DeviceRemove_sd1(void);
UINT8 Flash_Identify(UINT8 tLUN);

#ifndef TEST_HD
// code = 12h, Inquiry
static UINT8 InquiryID[36] = {
0x00, 0x80, 0x00, 0x01, 0x1F, 0x00, 0x00, 0x00,
'W', '9', '0', 'X', '9', '0', '0', 0x20,
'U', 'S', 'B', 0x20, 'C', 'a', 'r', 'd',
0x20, 'R', 'e', 'a', 'd', 'e', 'r', 0x20,
0x31, 0x2E, 0x30, 0x30 };
#else
static UINT8 InquiryID[36] = {
0x00, 0x00, 0x00, 0x00, 0x20, 0x00, 0x00, 0x00,
'W', '9', '0', 'X', '9', '0', '0', 0x20,
'U', 'S', 'B', 0x20, 'C', 'a', 'r', 'd',
0x20, 'R', 'e', 'a', 'd', 'e', 'r', 0x20,
0x31, 0x2E, 0x30, 0x30 };
#endif

// code = 5Ah, Mode Sense
static UINT8 Mode_Page_01[12] = {
	0x01, 0x0A, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00,
	0x03, 0x00, 0x00, 0x00 };

static UINT8 Mode_Page_05[32] = {
	0x05, 0x1E, 0x13, 0x88, 0x08, 0x20, 0x02, 0x00,
	0x01, 0xF4, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x05, 0x1E, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x01, 0x68, 0x00, 0x00 };

static UINT8 Mode_Page_1B[12] = {
	0x1B, 0x0A, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00 };

static UINT8 Mode_Page_1C[8] = {
	0x1C, 0x06, 0x00, 0x05, 0x00, 0x00, 0x00, 0x00 };

static UINT8 Mode_Page[24] = {
	0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x02, 0x00, 0x1C, 0x0A, 0x80, 0x03,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01 };
	
UINT8 volatile SenseKey = 0x00;
UINT8 volatile ASC= 0x00;
UINT8 volatile ASCQ=0x00;

// HHWu addb    2007.12.04
typedef struct request_sense
{
	UINT8 volatile ucSenseKey;
	UINT8 volatile ucASC;
	UINT8 volatile ucASCQ;
} REQUEST_SENSE_T;

REQUEST_SENSE_T tSD0ReqSen, tSD1ReqSen;
// HHWu adde

typedef struct {
	UINT8	dCBWSignature[4]; 
	UINT8	dCBWTag[4];
	union
	{
		UINT8	c[4];
		UINT32	c32;
	} dCBWDataTferLh;         
	UINT8	bmCBWFlags;	
	UINT8	bCBWLUN;	
	UINT8	bCBWCBLength;
	UINT8	OP_Code;
	UINT8	LUN;
	union 
	{
		UINT8	d[4];
		__packed	UINT32	d32;
	} LBA;
	union
	{
		UINT8	f[4];
		__packed	UINT32	f32;
	} CBWCB_In; 
	UINT8	CBWCB_Reserved[6];
} CBW;

union {
	CBW		CBWD; 	
	UINT8	CBW_Array[31];
} CBW_In;     

typedef struct {
	UINT8	dCSWSignature[4];
	UINT8	dCSWTag[4];
	union
	{
		UINT8	g[4];
		UINT32	g32;
	} dCSWDataResidue;    
	UINT8	bCSWStatus;	
} CSW;

union {
	CSW		CSWD; 	
	UINT8	CSW_Array[13];
} CSW_In;


void MscReset(void)
{
	outpw(REG_USBD_EPA_RSP_SC,EP_HALT);
	outpw(REG_USBD_EPB_RSP_SC,EP_HALT);
	errorFlagCBW = 1;
	_usbd_haltep1 = HALT;	// in endpoint halt
	_usbd_haltep2 = HALT;	// in endpoint halt  

}

UINT8 validateCbw(void)
{
	if ((!((CBW_In.CBWD.dCBWSignature[0]==0x55)&&
		(CBW_In.CBWD.dCBWSignature[1]==0x53)&&
		(CBW_In.CBWD.dCBWSignature[2]==0x42)&&
		(CBW_In.CBWD.dCBWSignature[3]==0x43)))
		)
	{
		bulkonlycmd=0;
		MscReset();
		return 1;
	}
	else
		return 0;

}

void udcMassBulk(void)
{
	if (Bulk_First_Flag==0)
	{
		Bulk_First_Flag=1;
		ReceiveCBW(Mass_Base_Addr);
	}

#ifdef TEST_SD0
    if (sd0_card_Insert_flag==1)
    {
       Flash_Identify(F_SD0_LUN);
       sd0_card_Insert_flag=0;
    }
#endif
#ifdef TEST_SD1
    if (sd1_card_Insert_flag==1)
    {
       Flash_Identify(F_SD1_LUN);
       sd1_card_Insert_flag=0;
    }
#endif

    if (bulkonlycmd==1)
    {
        FshBuf2CBW();
        if (validateCbw())
		{
			return;
		}
        if ((CBW_In.CBWD.OP_Code == 0x2a) || (CBW_In.CBWD.OP_Code == 0xaa))
        {
            Wt10_Command();
            CSW2FshBuf();
        }
        else if ((CBW_In.CBWD.OP_Code == 0x28)|| (CBW_In.CBWD.OP_Code == 0xa8))
        {
            Rd10_Command();
            CSW2FshBuf();
        }
        else if (CBW_In.CBWD.OP_Code == 0x12)
        {
            Inquiry_Command();
            CSW2FshBuf();
        }else if (CBW_In.CBWD.OP_Code == 0x23)
        {
            RdFmtCap_Command();
            CSW2FshBuf();
        }else if (CBW_In.CBWD.OP_Code == 0x25)
        {
            RdCurCap_Command();
            CSW2FshBuf();
        }
        else if (CBW_In.CBWD.OP_Code == 0x5a)
        {
            ModeSense_Command();
            CSW2FshBuf();
        }
        else if (CBW_In.CBWD.OP_Code == 0x1a)
        {
            ModeSense6_Command();
            CSW2FshBuf();
        }
        else if (CBW_In.CBWD.OP_Code == 0x00)
        {
            CSW2FshBuf();
        }else if (CBW_In.CBWD.OP_Code == 0x1e)
        {
            if (CBW_In.CBW_Array[19]==1)
            {
                preventflag=1;
                SenseKey = 0x05;
                ASC= 0x24;
                ASCQ = 0x00;
            }
            else
                preventflag=0;
            CSW2FshBuf();
        }else if (CBW_In.CBWD.OP_Code == 0x03)
        {
            ReqSen_Command();
            CSW2FshBuf();
        }else if (CBW_In.CBWD.OP_Code == 0x2F)
        {
            CSW2FshBuf();
        }else if (CBW_In.CBWD.OP_Code == 0x1B)
        {
            CSW2FshBuf();
        }else if (CBW_In.CBWD.OP_Code == 0x55)
        {
            ModeSel_Command();
            CSW2FshBuf();
        }
        else
        {
            SenseKey = 0x05;
            ASC= 0x24;
            ASCQ = 0x00;
            CSW2FshBuf(); 
        }
        bulkonlycmd=0;
    }
}

void FshBuf2CBW(void)
{
	UINT32 i,j;

	for (i = 0 ; i < 17 ; i++)
		CBW_In.CBW_Array[i] = readb(Mass_Base_Addr+i);

	j = Mass_Base_Addr+20;
	for (i = 17 ; i < 21 ; i++)
		CBW_In.CBW_Array[i] = readb(j--);

	j = Mass_Base_Addr+24;
	for (i = 21 ; i < 25 ; i++)
		CBW_In.CBW_Array[i] = readb(j--);

	for (i = 25 ; i < 31 ; i++)
		CBW_In.CBW_Array[i] = readb(Mass_Base_Addr+i);
} 

void CSW2FshBuf(void)
{
	UINT8 i;

	for (i = 0 ; i < 13 ; i++)
		CSW_In.CSW_Array[i] = 0x00;

	CSW_In.CSWD.dCSWSignature[0] = 0x55;
	CSW_In.CSWD.dCSWSignature[1] = 0x53;
	CSW_In.CSWD.dCSWSignature[2] = 0x42;
	CSW_In.CSWD.dCSWSignature[3] = 0x53;

	for (i = 0 ; i < 4 ; i++)          
		CSW_In.CSWD.dCSWTag[i] = CBW_In.CBWD.dCBWTag[i];

    writeb(Mass_Base_Addr+12,0x00);

    CSW_In.CSWD.bCSWStatus = preventflag;

    for (i = 0 ; i < 13 ; i++)
        writeb(Mass_Base_Addr+i,CSW_In.CSW_Array[i]);

#ifdef TEST_SD0
    if (CBW_In.CBWD.bCBWLUN==F_SD0_LUN)
    {
        if ((Card_STS==FMI_SD_WRITE_PROTECT)&&(CBW_In.CBWD.OP_Code == 0x2a))
        {
            writeb(Mass_Base_Addr+12,0x01);
        }
        
        if (sd0_card_remove_flag==1)  
        {
            if((CBW_In.CBWD.OP_Code == 0x12)||(CBW_In.CBWD.OP_Code == 0x03))
                writeb(Mass_Base_Addr+12,0x00);
            else
                writeb(Mass_Base_Addr+12,0x01);
        }
    }
#endif

#ifdef TEST_SD1
    if (CBW_In.CBWD.bCBWLUN==F_SD1_LUN)
    {
        if ((Card_STS==FMI_SD_WRITE_PROTECT)&&(CBW_In.CBWD.OP_Code == 0x2a))
        {
            writeb(Mass_Base_Addr+12,0x01);
        }
        
        if (sd1_card_remove_flag==1)  
        {
            if((CBW_In.CBWD.OP_Code == 0x12)||(CBW_In.CBWD.OP_Code == 0x03))
                writeb(Mass_Base_Addr+12,0x00);
            else
                writeb(Mass_Base_Addr+12,0x01);
        }
    }
#endif

	SDRAM2USB_Bulk(Mass_Base_Addr, 0x0d);
	Bulk_First_Flag=0;
}

void Inquiry_Command(void)
{
    int i;
    UINT32 wd;

    wd = CBW_In.CBWD.dCBWDataTferLh.c32;
    for (i = 0 ; i < sizeof(InquiryID); i++)
        writeb(Mass_Base_Addr+i,InquiryID[i]);
    SDRAM2USB_Bulk(Mass_Base_Addr,wd);
}

void RdFmtCap_Command(void)
{
    int i;
    UINT32 rd;
    UINT32 tmpval;
     
    tmpval=Mass_Base_Addr;
    for (i = 0 ; i < 36 ; i++)
        writeb(tmpval+i,0x00);
    writeb(tmpval+3,0x10);
    
#ifdef TEST_RAM 
    writeb(tmpval+4,*((UINT8 *)&gTotalSectors_RAM+3));
    writeb(tmpval+5,*((UINT8 *)&gTotalSectors_RAM+2));
    writeb(tmpval+6,*((UINT8 *)&gTotalSectors_RAM+1));
    writeb(tmpval+7,*((UINT8 *)&gTotalSectors_RAM+0));
    writeb(tmpval+8,0x02);
    writeb(tmpval+10,0x02);
    writeb(tmpval+12,*((UINT8 *)&gTotalSectors_RAM+3));
    writeb(tmpval+13,*((UINT8 *)&gTotalSectors_RAM+2));
    writeb(tmpval+14,*((UINT8 *)&gTotalSectors_RAM+1));
    writeb(tmpval+15,*((UINT8 *)&gTotalSectors_RAM+0)); 
#endif    
#ifdef TEST_HD 
    if (CBW_In.CBWD.bCBWLUN==F_HD_LUN)
    {
        writeb(tmpval+4,*((UINT8 *)&gTotalSectors_HD+3));
        writeb(tmpval+5,*((UINT8 *)&gTotalSectors_HD+2));
        writeb(tmpval+6,*((UINT8 *)&gTotalSectors_HD+1));
        writeb(tmpval+7,*((UINT8 *)&gTotalSectors_HD+0));
        writeb(tmpval+8,0x02);
        writeb(tmpval+10,0x02);
        writeb(tmpval+12,*((UINT8 *)&gTotalSectors_HD+3));
        writeb(tmpval+13,*((UINT8 *)&gTotalSectors_HD+2));
        writeb(tmpval+14,*((UINT8 *)&gTotalSectors_HD+1));
        writeb(tmpval+15,*((UINT8 *)&gTotalSectors_HD+0));
    }
#endif
#ifdef TEST_SD0
    if (CBW_In.CBWD.bCBWLUN==F_SD0_LUN)
    {
        writeb(tmpval+4,*((UINT8 *)&gTotalSectors_SD0+3));
        writeb(tmpval+5,*((UINT8 *)&gTotalSectors_SD0+2));
        writeb(tmpval+6,*((UINT8 *)&gTotalSectors_SD0+1));
        writeb(tmpval+7,*((UINT8 *)&gTotalSectors_SD0+0));
        writeb(tmpval+8,0x02);
        writeb(tmpval+10,0x02);
        writeb(tmpval+12,*((UINT8 *)&gTotalSectors_SD0+3));
        writeb(tmpval+13,*((UINT8 *)&gTotalSectors_SD0+2));
        writeb(tmpval+14,*((UINT8 *)&gTotalSectors_SD0+1));
        writeb(tmpval+15,*((UINT8 *)&gTotalSectors_SD0+0));
    }
#endif
#ifdef TEST_SD1
    if (CBW_In.CBWD.bCBWLUN==F_SD1_LUN)
    {
        writeb(tmpval+4,*((UINT8 *)&gTotalSectors_SD1+3));
        writeb(tmpval+5,*((UINT8 *)&gTotalSectors_SD1+2));
        writeb(tmpval+6,*((UINT8 *)&gTotalSectors_SD1+1));
        writeb(tmpval+7,*((UINT8 *)&gTotalSectors_SD1+0));
        writeb(tmpval+8,0x02);
        writeb(tmpval+10,0x02);
        writeb(tmpval+12,*((UINT8 *)&gTotalSectors_SD1+3));
        writeb(tmpval+13,*((UINT8 *)&gTotalSectors_SD1+2));
        writeb(tmpval+14,*((UINT8 *)&gTotalSectors_SD1+1));
        writeb(tmpval+15,*((UINT8 *)&gTotalSectors_SD1+0));
    }
#endif
#ifdef TEST_MS0
    if (CBW_In.CBWD.bCBWLUN==F_MS0_LUN)
    {
        writeb(tmpval+4,*((UINT8 *)&gTotalSectors_MS0+3));
        writeb(tmpval+5,*((UINT8 *)&gTotalSectors_MS0+2));
        writeb(tmpval+6,*((UINT8 *)&gTotalSectors_MS0+1));
        writeb(tmpval+7,*((UINT8 *)&gTotalSectors_MS0+0));
        writeb(tmpval+8,0x02);
        writeb(tmpval+10,0x02);
        writeb(tmpval+12,*((UINT8 *)&gTotalSectors_MS0+3));
        writeb(tmpval+13,*((UINT8 *)&gTotalSectors_MS0+2));
        writeb(tmpval+14,*((UINT8 *)&gTotalSectors_MS0+1));
        writeb(tmpval+15,*((UINT8 *)&gTotalSectors_MS0+0));
    }
#endif
#ifdef TEST_MS1
    if (CBW_In.CBWD.bCBWLUN==F_MS1_LUN)
    {
        writeb(tmpval+4,*((UINT8 *)&gTotalSectors_MS1+3));
        writeb(tmpval+5,*((UINT8 *)&gTotalSectors_MS1+2));
        writeb(tmpval+6,*((UINT8 *)&gTotalSectors_MS1+1));
        writeb(tmpval+7,*((UINT8 *)&gTotalSectors_MS1+0));
        writeb(tmpval+8,0x02);
        writeb(tmpval+10,0x02);
        writeb(tmpval+12,*((UINT8 *)&gTotalSectors_MS1+3));
        writeb(tmpval+13,*((UINT8 *)&gTotalSectors_MS1+2));
        writeb(tmpval+14,*((UINT8 *)&gTotalSectors_MS1+1));
        writeb(tmpval+15,*((UINT8 *)&gTotalSectors_MS1+0));
    }
#endif
#ifdef TEST_SM
    if (CBW_In.CBWD.bCBWLUN==F_SM_LUN)
    {
        writeb(tmpval+4,*((UINT8 *)&gTotalSectors_SM+3));
        writeb(tmpval+5,*((UINT8 *)&gTotalSectors_SM+2));
        writeb(tmpval+6,*((UINT8 *)&gTotalSectors_SM+1));
        writeb(tmpval+7,*((UINT8 *)&gTotalSectors_SM+0));
        writeb(tmpval+8,0x02);
        writeb(tmpval+10,0x02);
        writeb(tmpval+12,*((UINT8 *)&gTotalSectors_SM+3));
        writeb(tmpval+13,*((UINT8 *)&gTotalSectors_SM+2));
        writeb(tmpval+14,*((UINT8 *)&gTotalSectors_SM+1));
        writeb(tmpval+15,*((UINT8 *)&gTotalSectors_SM+0));
    }
#endif

    writeb(tmpval+18,0x02);

    rd = CBW_In.CBWD.dCBWDataTferLh.c32;
    SDRAM2USB_Bulk(Mass_Base_Addr,rd);
}

void RdCurCap_Command(void)
{
    int i;
    UINT32 tmpval,tmpval1;
    
    tmpval=Mass_Base_Addr;
    for (i = 0 ; i < 36 ; i++)
        writeb(tmpval+i,0x00);

#ifdef TEST_RAM 
    tmpval1=gTotalSectors_RAM-1;
    writeb(tmpval,*((UINT8 *)&tmpval1+3));
    writeb(tmpval+1,*((UINT8 *)&tmpval1+2));
    writeb(tmpval+2,*((UINT8 *)&tmpval1+1));
    writeb(tmpval+3,*((UINT8 *)&tmpval1+0));
#endif 
#ifdef TEST_HD 
    if (CBW_In.CBWD.bCBWLUN==F_HD_LUN)
    {
        tmpval1=gTotalSectors_HD-1;
        writeb(tmpval,*((UINT8 *)&tmpval1+3));
        writeb(tmpval+1,*((UINT8 *)&tmpval1+2));
        writeb(tmpval+2,*((UINT8 *)&tmpval1+1));
        writeb(tmpval+3,*((UINT8 *)&tmpval1+0));
    }
#endif 
#ifdef TEST_SD0
    if (CBW_In.CBWD.bCBWLUN==F_SD0_LUN)
    {
        tmpval1=gTotalSectors_SD0-1;
        writeb(tmpval,*((UINT8 *)&tmpval1+3));
        writeb(tmpval+1,*((UINT8 *)&tmpval1+2));
        writeb(tmpval+2,*((UINT8 *)&tmpval1+1));
        writeb(tmpval+3,*((UINT8 *)&tmpval1+0));
    }
#endif
#ifdef TEST_SD1
    if (CBW_In.CBWD.bCBWLUN==F_SD1_LUN)
    {
        tmpval1=gTotalSectors_SD1-1;
        writeb(tmpval,*((UINT8 *)&tmpval1+3));
        writeb(tmpval+1,*((UINT8 *)&tmpval1+2));
        writeb(tmpval+2,*((UINT8 *)&tmpval1+1));
        writeb(tmpval+3,*((UINT8 *)&tmpval1+0));
    }
#endif
#ifdef TEST_MS0
    if (CBW_In.CBWD.bCBWLUN==F_MS0_LUN)
    {
        tmpval1=gTotalSectors_MS0-1;
        writeb(tmpval,*((UINT8 *)&tmpval1+3));
        writeb(tmpval+1,*((UINT8 *)&tmpval1+2));
        writeb(tmpval+2,*((UINT8 *)&tmpval1+1));
        writeb(tmpval+3,*((UINT8 *)&tmpval1+0));
    }
#endif
#ifdef TEST_MS1
    if (CBW_In.CBWD.bCBWLUN==F_MS1_LUN)
    {
        tmpval1=gTotalSectors_MS1-1;
        writeb(tmpval,*((UINT8 *)&tmpval1+3));
        writeb(tmpval+1,*((UINT8 *)&tmpval1+2));
        writeb(tmpval+2,*((UINT8 *)&tmpval1+1));
        writeb(tmpval+3,*((UINT8 *)&tmpval1+0));
    }
#endif
#ifdef TEST_SM
    if (CBW_In.CBWD.bCBWLUN==F_SM_LUN)
    {
        tmpval1=gTotalSectors_SM-1;
        writeb(tmpval,*((UINT8 *)&tmpval1+3));
        writeb(tmpval+1,*((UINT8 *)&tmpval1+2));
        writeb(tmpval+2,*((UINT8 *)&tmpval1+1));
        writeb(tmpval+3,*((UINT8 *)&tmpval1+0));
    }
#endif

    writeb(tmpval+6,0x02);
    SDRAM2USB_Bulk(Mass_Base_Addr,CBW_In.CBWD.dCBWDataTferLh.c32);
}

void Rd10_Command(void)
{
    UINT32 rd,lba1;
     
    rd = CBW_In.CBWD.dCBWDataTferLh.c32;
    lba1 = CBW_In.CBWD.LBA.d32;

#ifdef TEST_RAM
	rd = CBW_In.CBWD.dCBWDataTferLh.c32;
	lba1 = (CBW_In.CBWD.LBA.d32*SizePerSector) + Storage_Base_Addr;
	SDRAM2USB_Bulk(lba1, rd);
#endif
#ifdef TEST_HD
 	if (CBW_In.CBWD.bCBWLUN==F_HD_LUN)
 	{
		if ( !atapiRead( 0, (UINT32)Storage_Base_Addr, lba1, (rd/SizePerSector) ) )	
		    MDEBUG("read error\n");
	    
		SDRAM2USB_Bulk(Storage_Base_Addr,rd);   
	} 	
#endif
#ifdef TEST_SD0
    if (CBW_In.CBWD.bCBWLUN==F_SD0_LUN)
    {
        if (sd0_card_remove_flag==0)  
            Card_STS = sicSdRead(0, lba1, (rd/SizePerSector), (unsigned int)Storage_Base_Addr);
            
        SDRAM2USB_Bulk(Storage_Base_Addr,rd);
    }
#endif
#ifdef TEST_SD1
    if (CBW_In.CBWD.bCBWLUN==F_SD1_LUN)
    {
        if (sd1_card_remove_flag==0)
            Card_STS = sicSdRead(1, lba1, (rd/SizePerSector), (unsigned int)Storage_Base_Addr);
            
        SDRAM2USB_Bulk(Storage_Base_Addr,rd);
    }
#endif
#ifdef TEST_MS0
    if (CBW_In.CBWD.bCBWLUN==F_MS0_LUN)
    {
        if (no_card_flag==0)
            Card_STS = sicMsdRead(0, lba1, (rd/SizePerSector), (unsigned int)Storage_Base_Addr);
            
        SDRAM2USB_Bulk(Storage_Base_Addr,rd);
    }
#endif
#ifdef TEST_MS1
    if (CBW_In.CBWD.bCBWLUN==F_MS1_LUN)
    {
        if (no_card_flag==0)
            Card_STS = sicMsdRead(1, lba1, (rd/SizePerSector), (unsigned int)Storage_Base_Addr);
            
        SDRAM2USB_Bulk(Storage_Base_Addr,rd);
    }
#endif
#ifdef TEST_SM
    if (CBW_In.CBWD.bCBWLUN==F_SM_LUN)
    {
        if (no_card_flag==0)
            status = sicNandRead(0, lba1, (rd/SizePerSector), (unsigned int)Storage_Base_Addr);
            
        SDRAM2USB_Bulk(Storage_Base_Addr,rd);
    }
#endif

    rd=0;
}


void Wt10_Command(void)
{
    UINT32 rd,lba1;
     
    rd = CBW_In.CBWD.dCBWDataTferLh.c32;
    lba1 = CBW_In.CBWD.LBA.d32;
    
#ifdef TEST_RAM    
    rd = CBW_In.CBWD.dCBWDataTferLh.c32;
	lba1 = (CBW_In.CBWD.LBA.d32*SizePerSector) + Storage_Base_Addr;
	USB2SDRAM_Bulk(lba1, rd);
#endif
#ifdef TEST_HD  
    if (CBW_In.CBWD.bCBWLUN==F_HD_LUN)
    {    
        USB2SDRAM_Bulk(Storage_Base_Addr,rd);		
        if ( !atapiWrite( 0, Storage_Base_Addr, lba1, (rd/SizePerSector) ) )
		    MDEBUG("write error\n");
	}
#endif
#ifdef TEST_SD0
    if (CBW_In.CBWD.bCBWLUN==F_SD0_LUN)
    {
        USB2SDRAM_Bulk(Storage_Base_Addr,rd);
        if (sd0_card_remove_flag==0)  
        {
            Card_STS = sicSdWrite(0, lba1, (rd/SizePerSector), (unsigned int)Storage_Base_Addr);
            if (Card_STS==FMI_SD_WRITE_PROTECT)
            {
                tSD0ReqSen.ucSenseKey = 0x07;   
	            tSD0ReqSen.ucASC= 0x27;
	            tSD0ReqSen.ucASCQ = 0x00;
            }
        }
    }
#endif
#ifdef TEST_SD1
    if (CBW_In.CBWD.bCBWLUN==F_SD1_LUN)
    {
        USB2SDRAM_Bulk(Storage_Base_Addr,rd);
        if (sd1_card_remove_flag==0)
        {
            Card_STS = sicSdWrite(1, lba1, (rd/SizePerSector), (unsigned int)Storage_Base_Addr);
            if (Card_STS==FMI_SD_WRITE_PROTECT)
            {
                tSD1ReqSen.ucSenseKey = 0x07;   
	            tSD1ReqSen.ucASC= 0x27;
	            tSD1ReqSen.ucASCQ = 0x00;
            }
        }
    }
#endif
#ifdef TEST_MS0
    if (CBW_In.CBWD.bCBWLUN==F_MS0_LUN)
    {
        USB2SDRAM_Bulk(Storage_Base_Addr,rd);
        if (no_card_flag==0)
        {
            Card_STS = sicMsWrite(0, lba1, (rd/SizePerSector), (unsigned int)Storage_Base_Addr);
            
            if (Card_STS==FMI_SD_WRITE_PROTECT)
            {
                SenseKey = 0x07;
                ASC= 0x27;
                ASCQ = 0x00;
            }
        }
    }
#endif
#ifdef TEST_MS1
    if (CBW_In.CBWD.bCBWLUN==F_MS1_LUN)
    {
        USB2SDRAM_Bulk(Storage_Base_Addr,rd);
        if (no_card_flag==0)
        {
            Card_STS = sicMsWrite(1, lba1, (rd/SizePerSector), (unsigned int)Storage_Base_Addr);
            
            if (Card_STS==FMI_SD_WRITE_PROTECT)
            {
                SenseKey = 0x07;
                ASC= 0x27;
                ASCQ = 0x00;
            }
        }
    }
#endif
#ifdef TEST_SM
    if (CBW_In.CBWD.bCBWLUN==F_SM_LUN)
    {
        USB2SDRAM_Bulk(Storage_Base_Addr,rd);
        Card_STS =  sicNandWrite(0, lba1, (rd/SizePerSector), (unsigned int)Storage_Base_Addr);
    }
#endif

    rd=0;
}

void ModeSense10_Command(void)
{
	UINT8 i,j;

	for (i = 0 ; i < 8 ; i++)
		writeb(Mass_Base_Addr+i, 0x00);

	switch (CBW_In.CBWD.LBA.d[0])
	{
		case 0x01:
			writeb(Mass_Base_Addr,19);
			i = 8;
			for (j = 0; j<12; j++)
			{
				writeb(Mass_Base_Addr+i,Mode_Page_01[j]);
				i++;
			}
			break;

		case 0x05:
			writeb(Mass_Base_Addr, 39);
			i = 8;
			for (j = 0; j<32; j++)
			{
				writeb(Mass_Base_Addr+i, Mode_Page_05[j]);
				i++;
			}
			writeb(Mass_Base_Addr+12, DDB.NumHead);
			writeb(Mass_Base_Addr+13, DDB.NumSector);
			writeb(Mass_Base_Addr+16, (UINT8)(DDB.NumCyl >> 8));
			writeb(Mass_Base_Addr+17, (UINT8)(DDB.NumCyl & 0x00ff));
			break;

		case 0x1B:
			writeb(Mass_Base_Addr,19);
			i = 8;
			for (j = 0; j<12; j++)
			{
				writeb(Mass_Base_Addr+i,Mode_Page_1B[j]);
				i++;
			}
			break;

		case 0x1C:
			writeb(Mass_Base_Addr,15);
			i = 8;
			for (j = 0; j<8; j++)
			{
				writeb(Mass_Base_Addr+i,Mode_Page_1C[j]);
				i++;
			}
			break;

		case 0x3F:
			writeb(Mass_Base_Addr, 0x47);
			i = 8;
			for (j = 0; j<12; j++)
			{
				writeb(Mass_Base_Addr+i, Mode_Page_01[j]);
				i++;
			}

			for (j = 0; j<32; j++)
			{
				writeb(Mass_Base_Addr+i, Mode_Page_05[j]);
				i++;
			}

			for (j = 0; j<12; j++)
			{
				writeb(Mass_Base_Addr+i, Mode_Page_1B[j]);
				i++;
			}

			for (j = 0; j<8; j++)
			{
				writeb(Mass_Base_Addr+i, Mode_Page_1C[j]);
				i++;
			}
			writeb(Mass_Base_Addr+24, DDB.NumHead);
			writeb(Mass_Base_Addr+25, DDB.NumSector);
			writeb(Mass_Base_Addr+28, (UINT8)(DDB.NumCyl >> 8));
			writeb(Mass_Base_Addr+29, (UINT8)(DDB.NumCyl & 0x00ff));
			break;

		default:
			SenseKey = 0x05;  //INVALID COMMAND
			ASC = 0x24;
			ASCQ = 0;
	}
	SDRAM2USB_Bulk(Mass_Base_Addr, CBW_In.CBWD.dCBWDataTferLh.c32);
}

void ModeSense6_Command(void)
{
	UINT8 i;

	for (i = 0; i<4; i++)
		writeb(Mass_Base_Addr+i,Mode_Page[i]);

	SDRAM2USB_Bulk(Mass_Base_Addr,CBW_In.CBWD.dCBWDataTferLh.c32);
}

void ReqSen_Command(void)
{
    UINT8 i;

    for (i = 0 ; i < 18 ; i++)
        writeb(Mass_Base_Addr+i,0x00);
    if (preventflag==1)
    {
        preventflag=0;
        writeb(Mass_Base_Addr,0x70);
    }
    else
        writeb(Mass_Base_Addr,0xf0);

#ifdef TEST_SD0
    if (CBW_In.CBWD.bCBWLUN==F_SD0_LUN)
    {
    	if (sd0_card_remove_flag==1)
        {
            /* 
            	If send 0x06 0x28 0x00 to host, the situation is strange that PC can't show
            	"insert disk" when no card inserted.            	
            	If send 0x02 0x3a 0x00 to host, everything is fine.            	
            */
            tSD0ReqSen.ucSenseKey = 0x02;   
            tSD0ReqSen.ucASC= 0x3a;
            tSD0ReqSen.ucASCQ = 0x00;
        }
        
        writeb(Mass_Base_Addr+2,tSD0ReqSen.ucSenseKey);
	    writeb(Mass_Base_Addr+7,0x0a);
	    writeb(Mass_Base_Addr+12,tSD0ReqSen.ucASC);
	    writeb(Mass_Base_Addr+13,tSD0ReqSen.ucASCQ);
	    SDRAM2USB_Bulk(Mass_Base_Addr,CBW_In.CBWD.dCBWDataTferLh.c32);
	    return;
    }
#endif

#ifdef TEST_SD1
    if (CBW_In.CBWD.bCBWLUN==F_SD1_LUN)
    {
    	if (sd1_card_remove_flag==1)
        {
        	tSD1ReqSen.ucSenseKey = 0x02;   
            tSD1ReqSen.ucASC= 0x3a;
            tSD1ReqSen.ucASCQ = 0x00;
        }
        
        writeb(Mass_Base_Addr+2,tSD1ReqSen.ucSenseKey);
	    writeb(Mass_Base_Addr+7,0x0a);
	    writeb(Mass_Base_Addr+12,tSD1ReqSen.ucASC);
	    writeb(Mass_Base_Addr+13,tSD1ReqSen.ucASCQ);
	    SDRAM2USB_Bulk(Mass_Base_Addr,CBW_In.CBWD.dCBWDataTferLh.c32);
	    return;
    }
#endif

    writeb(Mass_Base_Addr+2,SenseKey);
    writeb(Mass_Base_Addr+7,0x0a);
    writeb(Mass_Base_Addr+12,ASC);
    writeb(Mass_Base_Addr+13,ASCQ);
    SDRAM2USB_Bulk(Mass_Base_Addr,CBW_In.CBWD.dCBWDataTferLh.c32);
    SenseKey = 0;
    ASC= 0;
    ASCQ = 0;
}

void ModeSel_Command(void)
{
    USB2SDRAM_Bulk(Storage_Base_Addr,CBW_In.CBWD.dCBWDataTferLh.c32);
}

void ModeSense_Command(void)
{
    UINT8 i,j;
	UINT8 NumHead,NumSector, NumCyl;

	for (i = 0 ; i < 8 ; i++)
		writeb(Mass_Base_Addr+i, 0x00);

/*
	if (CBW_In.CBWD.bCBWLUN==F_SD0_LUN)
	{
		if (_fmi_bIsSDWriteProtect == TRUE)
			writeb(Mass_Base_Addr+3,0x80);
	}
	if (CBW_In.CBWD.bCBWLUN==F_SD1_LUN)
	{
		if (_fmi_bIsSDWriteProtect == TRUE)
			writeb(Mass_Base_Addr+3,0x80);
	}
*/

	switch (CBW_In.CBWD.LBA.d[3])
	{
		case 0x01:
			writeb(Mass_Base_Addr+1,18);
			for (j = 0; j<12; j++)
				writeb(Mass_Base_Addr+8+j,Mode_Page_01[j]);
			break;

		case 0x05:
			writeb(Mass_Base_Addr+1,38);
			for (j = 0; j<32; j++)
				writeb(Mass_Base_Addr+8+j, Mode_Page_05[j]);
			NumHead = 2;
			NumSector = 64;

#ifdef TEST_HD
			if (CBW_In.CBWD.bCBWLUN==F_HD_LUN)
			    NumCyl = gTotalSectors_HD/128;
#endif
#ifdef TEST_SD0
			if (CBW_In.CBWD.bCBWLUN==F_SD0_LUN)
			NumCyl = gTotalSectors_SD0/128;
#endif
#ifdef TEST_SD1
			if (CBW_In.CBWD.bCBWLUN==F_SD1_LUN)
			NumCyl = gTotalSectors_SD1/128;
#endif
#ifdef TEST_MS0
			if (CBW_In.CBWD.bCBWLUN==F_MS0_LUN)
			NumCyl = gTotalSectors_MS0/128;
#endif
#ifdef TEST_MS1
			if (CBW_In.CBWD.bCBWLUN==F_MS1_LUN)
			NumCyl = gTotalSectors_MS1/128;
#endif
#ifdef TEST_SM
			if (CBW_In.CBWD.bCBWLUN==F_SM_LUN)
			NumCyl = gTotalSectors_SM/128;
#endif

#ifdef TEST_RAM
			writeb(Mass_Base_Addr+12, DDB.NumHead);
			writeb(Mass_Base_Addr+13, DDB.NumSector);
			writeb(Mass_Base_Addr+16, (UINT8)(DDB.NumCyl >> 8));
			writeb(Mass_Base_Addr+17, (UINT8)(DDB.NumCyl & 0x00ff));
#else
			writeb(Mass_Base_Addr+12,NumHead);
			writeb(Mass_Base_Addr+13,NumSector);
			writeb(Mass_Base_Addr+16,(UINT8)(NumCyl >> 8));
			writeb(Mass_Base_Addr+17,(UINT8)(NumCyl & 0x00ff));
#endif			
			break;

		case 0x1B:
			writeb(Mass_Base_Addr+1,18);
			for (j = 0; j<12; j++)
				writeb(Mass_Base_Addr+8+j, Mode_Page_1B[j]);
			break;

		case 0x1C:
			writeb(Mass_Base_Addr+1,14);
			for (j = 0; j<8; j++)
				writeb(Mass_Base_Addr+8+j, Mode_Page_1C[j]);
			break;

		case 0x3F:
			writeb(Mass_Base_Addr+1,0x46);
			for (j = 0; j<12; j++)
				writeb(Mass_Base_Addr+8+j, Mode_Page_01[j]);
			for (j = 0; j<32; j++)
				writeb(Mass_Base_Addr+20+j, Mode_Page_05[j]);
			for (j = 0; j<12; j++)
				writeb(Mass_Base_Addr+52+j, Mode_Page_1B[j]);
			for (j = 0; j<8; j++)
				writeb(Mass_Base_Addr+64+j, Mode_Page_1C[j]);
			NumHead = 2;
			NumSector = 64;

#ifdef TEST_HD
			if (CBW_In.CBWD.bCBWLUN==F_HD_LUN)
			    NumCyl = gTotalSectors_HD/128;
#endif
#ifdef TEST_SD0
			if (CBW_In.CBWD.bCBWLUN==F_SD0_LUN)
			NumCyl = gTotalSectors_SD0/128;
#endif
#ifdef TEST_SD1
			if (CBW_In.CBWD.bCBWLUN==F_SD1_LUN)
			NumCyl = gTotalSectors_SD1/128;
#endif
#ifdef TEST_MS0
			if (CBW_In.CBWD.bCBWLUN==F_MS0_LUN)
			NumCyl = gTotalSectors_MS0/128;
#endif
#ifdef TEST_MS1
			if (CBW_In.CBWD.bCBWLUN==F_MS1_LUN)
			NumCyl = gTotalSectors_MS1/128;
#endif
#ifdef TEST_SM
			if (CBW_In.CBWD.bCBWLUN==F_SM_LUN)
			NumCyl = gTotalSectors_SM/128;
#endif

#ifdef TEST_RAM
			writeb(Mass_Base_Addr+24, DDB.NumHead);
			writeb(Mass_Base_Addr+25, DDB.NumSector);
			writeb(Mass_Base_Addr+28, (UINT8)(DDB.NumCyl >> 8));
			writeb(Mass_Base_Addr+29, (UINT8)(DDB.NumCyl & 0x00ff));
#else
			writeb(Mass_Base_Addr+24,NumHead);
			writeb(Mass_Base_Addr+25,NumSector);
			writeb(Mass_Base_Addr+28,(UINT8)(NumCyl >> 8));
			writeb(Mass_Base_Addr+29,(UINT8)(NumCyl & 0x00ff));
#endif			
			break;

		default:
			SenseKey = 0x05;  //INVALID COMMAND
			ASC = 0x24;
			ASCQ = 0;
	}
	SDRAM2USB_Bulk(Mass_Base_Addr,CBW_In.CBWD.dCBWDataTferLh.c32);
}

UINT8 udcFlashInit(void)
{
    F_SD0_LUN=0xff;
    F_SD1_LUN=0xff;
    F_MS0_LUN=0xff;
    F_MS1_LUN=0xff;
    F_SM_LUN=0xff;
    F_HD_LUN = 0xff;

#ifdef TEST_HD
    F_HD_LUN=Mass_LUN;
    Mass_LUN++;
     
    if (atapiInit(CLK_66MHZ))
	{
		UINT32  sector, udma_mode;		
		
       	atapiIoctl(ATAPI_MASTER, ATAPI_GET_CAPACITY, (UINT32)&sector);
        MDEBUG("\nMaste HD Total sectors : %d\n",sector);
        MDEBUG("capacity : %2.2fG\n",sector/2000000.0);
       
        atapiIoctl(ATAPI_MASTER, ATAPI_GET_UDMAMODE_INFO, (UINT32)&udma_mode);
        MDEBUG("UDMA mode=%x\n",udma_mode);
	}
	else
	{
	    printf("HD not found\n");
	    return 0;
    }
		
	if(!atapiOpen())
     	return 0;	
			
    if (!Flash_Identify(F_HD_LUN))
        return 0;
#endif
#ifdef TEST_SD0
    F_SD0_LUN=Mass_LUN;
    Mass_LUN++;
    fmiSetCallBack(FMI_SD_CARD0,(PVOID)DeviceRemove_sd0,(PVOID)DeviceInsert_sd0);
#endif

#ifdef TEST_SD1
    F_SD1_LUN=Mass_LUN;
    Mass_LUN++;
    fmiSetCallBack(FMI_SD_CARD1,(PVOID)DeviceRemove_sd1,(PVOID)DeviceInsert_sd1);
#endif

#ifdef TEST_MS0
    F_MS0_LUN=Mass_LUN;
    Mass_LUN++;
#endif

#ifdef TEST_MS1
    F_MS1_LUN=Mass_LUN;
    Mass_LUN++;
#endif

#ifdef TEST_SM
    F_SM_LUN=Mass_LUN;
    Mass_LUN++;
#endif

#ifndef TEST_RAM  /* compile issue */
    fmiInitDevice();
#endif    

#ifdef TEST_SD0
    if (!Flash_Identify(F_SD0_LUN))
        //return 0;
        ;
#endif
#ifdef TEST_SD1
    if (!Flash_Identify(F_SD1_LUN))
        //return 0;
        ;
#endif
#ifdef TEST_MS0
    if (!Flash_Identify(F_MS0_LUN))
        return 0;
#endif
#ifdef TEST_MS1
    if (!Flash_Identify(F_MS1_LUN))
        return 0;
#endif
#ifdef TEST_SM
    if (!Flash_Identify(F_SM_LUN))
        return 0;
#endif

#ifdef TEST_RAM
	DRAM_Identify(1);
#endif

    return 1;
}

#ifdef TEST_SD0
void DeviceInsert_sd0(void)
{
    UINT32 tmp=0x100000;
    
    sd0_card_remove_flag=0;
    sd0_card_Insert_flag=1;
    //Flash_Identify(F_SD0_LUN);  // ..... strange ? must be called in udcMassBulk()   
    while (tmp-- > 0);        
    
    //printf("SD0 insert!\n");
}

void DeviceRemove_sd0(void)
{
    sd0_card_remove_flag=1;
    sd0_card_Insert_flag=0;   
     
    //printf("SD0 remove!\n");
}
#endif

#ifdef TEST_SD1
void DeviceInsert_sd1(void)
{
    UINT32 tmp=0x100000;

    sd1_card_remove_flag=0;
    sd1_card_Insert_flag=1;
    while (tmp-- > 0);
    
    //printf("SD1 insert!\n");
}

void DeviceRemove_sd1(void)
{
    sd1_card_remove_flag=1;
    sd1_card_Insert_flag=0;

    //printf("SD1 remove!\n");
}
#endif

UINT8 Flash_Identify(UINT8 tLUN)
{
#ifdef TEST_HD
    if (tLUN==F_HD_LUN)
    {
        atapiIoctl(ATAPI_MASTER, ATAPI_GET_CAPACITY, (UINT32)&gTotalSectors_HD);
        MDEBUG("HD sectors = %d\n",gTotalSectors_HD);
        if (gTotalSectors_HD < 0)
            MDEBUG("gTotalSectors < 0\n");
    }
#endif   
#ifdef TEST_SD0
    if (tLUN==F_SD0_LUN)
    {
        gTotalSectors_SD0 = sicSdOpen(0);
        if (gTotalSectors_SD0 == FMI_NO_SD_CARD)
            sd0_card_remove_flag=1;  

        if (gTotalSectors_SD0 < 0)
        {
            /* cannot read format */
            tSD0ReqSen.ucSenseKey = 0x03;   
            tSD0ReqSen.ucASC= 0x30;
            tSD0ReqSen.ucASCQ = 0x01;            
            return 0;  
        }
    }
#endif
#ifdef TEST_SD1
	if (tLUN==F_SD1_LUN)
    {
        gTotalSectors_SD1 = sicSdOpen(1);
        if (gTotalSectors_SD1 == FMI_NO_SD_CARD)
            sd1_card_remove_flag=1;
        
        if (gTotalSectors_SD1 < 0)
        {
            /* cannot read format */
            tSD1ReqSen.ucSenseKey = 0x03;   
            tSD1ReqSen.ucASC= 0x30;
            tSD1ReqSen.ucASCQ = 0x01;
            return 0;
        }
    }
#endif
#ifdef TEST_MS0
    if (tLUN==F_MS0_LUN)
    {
        gTotalSectors_MS0 = sicMsOpen(0);
        if (gTotalSectors_MS0 < 0)
        {
            SenseKey = 0x03;   //cannot read format 
            ASC= 0x30;
            ASCQ = 0x01;
            return 0;
        }
    }
#endif
#ifdef TEST_MS1
    if (tLUN==F_MS1_LUN)
    {
        gTotalSectors_MS1 = sicMsOpen(1);
        if (gTotalSectors_MS1 < 0)
        {
            SenseKey = 0x03;   //cannot read format 
            ASC= 0x30;
            ASCQ = 0x01;
            return 0;
        }
    }
#endif
#ifdef TEST_SM
    if (tLUN==F_SM_LUN)
    {
        gTotalSectors_SM = sicNandOpen(0);
        if (gTotalSectors_SM < 0)
        {
            SenseKey = 0x03;   //cannot read format 
            ASC= 0x30;
            ASCQ = 0x01;
            return 0;
        }
    }
#endif


    return 1;	      // success
}

#ifdef TEST_RAM
void DRAM_Identify(UINT8 cap)
{
	if (cap==1)
	{
		gTotalSectors_RAM=1000;
	}

#if 0
	if (cap==2) 
	{
		gTotalSectors_RAM=8000;
		DDB.capacity = M_4M;
		DDB.NumCyl=250;
		DDB.NumHead=4;
		DDB.NumSector=8;
	}
	else if (cap==3)
	{
		gTotalSectors_RAM=16000;
		DDB.capacity = M_8M;
		DDB.NumCyl=250;
		DDB.NumHead=4;
		DDB.NumSector=16;
	}
	else if (cap==4) 
	{
		gTotalSectors_RAM=32000;
		DDB.capacity = M_16M;
		DDB.NumCyl=500;
		DDB.NumHead=4;
		DDB.NumSector=16;
	}
	else if (cap==5)
	{
		gTotalSectors_RAM=64000;
		DDB.capacity = M_32M;
		DDB.NumCyl=500;
		DDB.NumHead=8;
		DDB.NumSector=16;
	}
	else if (cap==6)
	{
		gTotalSectors_RAM=128000;
		DDB.capacity = M_64M;
		DDB.NumCyl=500;
		DDB.NumHead=8;
		DDB.NumSector=32;
	}
	else if (cap==7)
	{
		gTotalSectors_RAM=2560000;
		DDB.capacity = M_128M;
		DDB.NumCyl=500;
		DDB.NumHead=16;
		DDB.NumSector=32;
	}
#endif
}

#if 0
UINT8 format(void)
{
	UINT8 i;
	UINT8 *bp,fat16=0;
	UINT32 totalcluster;

	memset(Flash_Buffer, 0, SizePerSector);
	Flash_Buffer[0x1be] = 0x80;
	Flash_Buffer[0x1bf] = StartSect[DDB.capacity]/DDB.NumSector;
	Flash_Buffer[0x1c0] = (StartSect[DDB.capacity]%DDB.NumSector)+1;
	Flash_Buffer[0x1c1] = 0x00; 

	if (DDB.capacity>=7)
	{
		fat16=1;
		Flash_Buffer[0x1c2] = 0x06;
	}
	else
		Flash_Buffer[0x1c2] = 0x01;

	Flash_Buffer[0x1c3] = DDB.NumHead-1;
	Flash_Buffer[0x1c4] = ((DDB.NumCyl & 0x300)>>2)+DDB.NumSector;
	Flash_Buffer[0x1c5] = (DDB.NumCyl & 0xff)-1;
	Flash_Buffer[0x1c6] = StartSect[DDB.capacity];

	DDB.partition_size = gTotalSectors_RAM - StartSect[DDB.capacity];

	totalcluster = (UINT16)DDB.NumHead*DDB.NumCyl;
	bp = Flash_Buffer+0x1ca;
	put_uint32((UINT32)DDB.partition_size, (UINT8 **)&bp);

	Flash_Buffer[510] = 0x55;
	Flash_Buffer[511] = 0xaa;

	Write_Sector(0, Flash_Buffer);

	if (!WriteBootSector(fat16))
		return 0;

	memset(Flash_Buffer, 0, SizePerSector);
	for (i=0;i<FATSects[DDB.capacity]-1;i++)
	{
		Write_Sector((UINT32)(StartSect[DDB.capacity]+2+i),Flash_Buffer);
		Write_Sector((UINT32)(StartSect[DDB.capacity]+FATSects[DDB.capacity]+2+i),Flash_Buffer);
	}

	for (i=0;i<16;i++)      
	{
		Write_Sector((UINT32)(StartSect[DDB.capacity]+FATSects[DDB.capacity]*2+i+1),Flash_Buffer);
	}

	memset(Flash_Buffer, 0, SizePerSector);
	Flash_Buffer[0]=0xf8;

	Flash_Buffer[1] = 0xff;
	Flash_Buffer[2] = 0xff;
	if (fat16==1)
		Flash_Buffer[3] = 0xff;

	Write_Sector((UINT32)(StartSect[DDB.capacity]+1),Flash_Buffer);
	Write_Sector((UINT32)(StartSect[DDB.capacity]+1+FATSects[DDB.capacity]),Flash_Buffer);
	return 1;
}

UINT8 Write_Sector(UINT32 sector,UINT8 *buffer)
{
	UINT16 tmpv;
	UINT32 tmpv1;
	UINT8 gValue;

	tmpv1 = Storage_Base_Addr+(sector<<9);
	for (tmpv=0;tmpv<SizePerSector;tmpv++)
	{
		gValue=*buffer++;
		writeb(tmpv1,gValue);
		tmpv1++;
	}
	return 1;
}

UINT8 WriteBootSector(UINT8 fat16)
{
	UINT8 *bp;
	UINT8 i;

	memset(Flash_Buffer, 0, SizePerSector);
	for (i=0;i<0x13;i++)
		Flash_Buffer[i] = PartBootSector[i];

	if ((DDB.capacity==2) || (DDB.capacity==3))
		Flash_Buffer[13] = 16;
	else
		Flash_Buffer[13] = 32;

	bp = Flash_Buffer+0x13;
	if (DDB.partition_size < 0x10000)
		put_uint16(DDB.partition_size, (UINT8 **)&bp);
	else
		bp = bp+2;

	Flash_Buffer[0x15] = 0xf8;
	bp++;

	put_uint16(FATSects[DDB.capacity], (UINT8 **)&bp);
	put_uint16(DDB.NumSector, (UINT8 **)&bp);
	put_uint16(DDB.NumHead, (UINT8 **)&bp);

	Flash_Buffer[0x1c] = StartSect[DDB.capacity];

	if (DDB.partition_size >= 0x10000)
	{
		bp = Flash_Buffer+0x20;
		put_uint32(DDB.partition_size, (UINT8 **)&bp);
	}

	Flash_Buffer[0x36] = 'F';
	Flash_Buffer[0x37] = 'A';
	Flash_Buffer[0x38] = 'T';
	Flash_Buffer[0x39] = '1';
	if (fat16==1)
		Flash_Buffer[0x3a] = '6';
	else
		Flash_Buffer[0x3a] = '2';
	Flash_Buffer[0x3b] = ' ';
	Flash_Buffer[0x3c] = ' ';

	Flash_Buffer[510] = 0x55;
	Flash_Buffer[511] = 0xaa;

	Write_Sector((UINT32)(StartSect[DDB.capacity]), Flash_Buffer);
	return 1;
}

void put_uint32(UINT32 value, UINT8 **p)
{
	union
	{
		UINT8 c[4];
		UINT32 i32; 
	}un_32;

	un_32.i32 = value;
	**p = un_32.c[0];
	(*p)[1] = un_32.c[1];
	(*p)[2] = un_32.c[2];
	(*p)[3] = un_32.c[3];
}

void put_uint16(UINT16 value, UINT8 **p)
{
	*(*p)++ = (UINT8)(value & 0xff);
	*(*p)++ = (UINT8)(value >> 8);
}
#endif

#endif 
